#include <cv.h>
#include <highgui.h>
#include <cxcore.h>
#include <stdio.h>
#include <math.h>


//normal vector and distance to the origin both normalized
struct Plane {
	CvPoint3D32f normal;
	float d;
};


float dotProduct (CvPoint3D32f a, CvPoint3D32f b){
	float result;
	result = a.x*b.x + a.y*b.y + a.z*b.z;
	return result;
};

CvPoint3D32f crossProduct (CvPoint3D32f a, CvPoint3D32f b) {
	CvPoint3D32f result;
	result.x = a.y*b.z - a.z*b.y;
	result.y = -(a.x*b.z + a.z*b.y);
	result.z = a.x*b.y - a.y*b.x;
	return result;
};

float length (CvPoint3D32f a){
	float result;
	result = sqrt(dotProduct(a,a));
	return result;
};



CvPoint3D32f substract (CvPoint3D32f a, CvPoint3D32f b) {
	CvPoint3D32f result;
	result.x = a.x-b.x;
	result.y = a.y-b.y;
	result.z = a.z-b.z;
	return result;
};

CvPoint3D32f devide (CvPoint3D32f a, float x) {
	CvPoint3D32f result;
	result.x = a.x/x;
	result.y = a.y/x;
	result.z = a.z/x;
	return result;
};

/*CvPoint3D32f reflection(Plane plane, CvPoint3D32f dir, CvPoint3D32f collision){
 
};*/


Plane planeFromPoints(CvPoint3D32f a, CvPoint3D32f b, CvPoint3D32f c){
	Plane result;
	CvPoint3D32f r0 = substract(a,b);
	CvPoint3D32f r1 = substract(a,c);
	result.normal = crossProduct(r0,r1);
	float l = length(result.normal);
	result.normal = devide(result.normal, l);
	result.d = dotProduct(a, result.normal)/l;
	return result;
};


